#include "rtps/discovery/SEDPAgent.h"
#include "rtps/discovery/TopicData.h"
#include "rtps/entities/Participant.h"
#include "rtps/entities/Reader.h"
#include "rtps/entities/Writer.h"
#include "rtps/messages/MessageTypes.h"
#include "rtps/utils/Log.h"
#include "ucdr/microcdr.h"
#include "lwip/sys.h"

#if SEDP_VERBOSE && RTPS_GLOBAL_VERBOSE
#include "rtps/utils/printutils.h"
#define SEDP_LOG(...)                                                          \
  if (true) {                                                                  \
    printf("[SEDP] ");                                                         \
    printf(__VA_ARGS__);                                                       \
    printf("\n");                                                              \
  }
#else
#define SEDP_LOG(...) //
#endif

void rtps_SEDPAgent_init(struct rtps_SEDPAgent *this,  struct rtps_Participant *part, const struct rtps_BuiltInEndpoints *endpoints)
{
    if(sys_mutex_new(&(this->m_mutex)) != ERR_OK) {
        SEDP_LOG("SEDPAgent failed to create mutex\n");
        return;
    }

    this->m_part = part;
    this->m_endpoints.spdpWriter = endpoints->spdpWriter;
    this->m_endpoints.spdpReader = endpoints->spdpReader;
    this->m_endpoints.sedpPubWriter = endpoints->sedpPubWriter;
    this->m_endpoints.sedpPubReader = endpoints->sedpPubReader;
    this->m_endpoints.sedpSubWriter = endpoints->sedpSubWriter;
    this->m_endpoints.sedpSubReader = endpoints->sedpSubReader;
    if(this->m_endpoints.sedpPubReader != NULL) {
        rtps_StatefulReader_registerCallback((this->m_endpoints).sedpPubReader, rtps_SEDPAgent_receiveCallbackPublisher, this);
    }
    if(this->m_endpoints.sedpSubReader != NULL) {
        rtps_StatefulReader_registerCallback((this->m_endpoints).sedpSubReader, rtps_SEDPAgent_receiveCallbackSubscriber, this);
    }
    this->m_unmatchedRemoteWriters = rtps_MemoryPool_init(sizeof(TopicDataCompressed), MAX_NUM_UNMATCHED_REMOTE_WRITERS);
    this->m_numUnmatchedRemoteWriters = 0;
    this->m_unmatchedRemoteReaders = rtps_MemoryPool_init(sizeof(TopicDataCompressed), MAX_NUM_UNMATCHED_REMOTE_READERS);
    this->m_numMatchedRemoteReaders = 0;
    this->mfp_onNewPublisherCallback = NULL;
    this->mfp_onNewSubscriberCallback = NULL;
    this->m_onNewPublisherArgs = NULL;
	this->m_onNewSubscriberArgs = NULL;
}

void rtps_SEDPAgent_deinit(struct rtps_SEDPAgent *this)
{
    rtps_MemoryPool_deinit(this->m_unmatchedRemoteWriters);
    rtps_MemoryPool_deinit(this->m_unmatchedRemoteReaders);
}

void rtps_SEDPAgent_registerOnNewPublisherMatchedCallback(struct rtps_SEDPAgent *this, void (*callback)(void *arg), void *args) {
    this->mfp_onNewPublisherCallback = callback;
    this->m_onNewPublisherArgs = args;
}

void rtps_SEDPAgent_registerOnNewSubscriberMatchedCallback(struct rtps_SEDPAgent *this, void (*callback)(void *arg), void *args) {
    this->mfp_onNewSubscriberCallback = callback;
    this->m_onNewSubscriberArgs = args;
}

void rtps_SEDPAgent_receiveCallbackPublisher(void *callee, const struct rtps_ReaderCacheChange *cacheChange) {
    struct rtps_SEDPAgent *agent = (struct rtps_SEDPAgent*)callee;
    rtps_SEDPAgent_onNewPublisher_ReaderCacheChange(agent, cacheChange);
}

void rtps_SEDPAgent_receiveCallbackSubscriber(void *callee, const struct rtps_ReaderCacheChange *cacheChange) {
    struct rtps_SEDPAgent *agent = (struct rtps_SEDPAgent*)callee;
    rtps_SEDPAgent_onNewSubscriber_ReaderCacheChange(agent, cacheChange);
}

void rtps_SEDPAgent_onNewPublisher_ReaderCacheChange(struct rtps_SEDPAgent *this, const struct rtps_ReaderCacheChange *change) {
    rtps_Lock(&this->m_mutex);
    
#if SEDP_VERBOSE
    SEDP_LOG("New publisher\n");
#endif

    if (!rtps_ReaderCacheChange_copyInto(change,this->m_buffer, sizeof(this->m_buffer) / sizeof((this->m_buffer)[0]))) {
#if SEDP_VERBOSE
        SEDP_LOG("EDPAgent: Buffer too small.\n");
#endif
        rtps_Unlock(&this->m_mutex);
        return;
    }
    struct ucdrBuffer cdrBuffer;
    ucdr_init_buffer(&cdrBuffer, this->m_buffer, sizeof(this->m_buffer));

    struct rtps_TopicData topicData;
    if (rtps_TopicData_readFromUcdrBuffer(&topicData, &cdrBuffer)) {
        rtps_SEDPAgent_onNewPublisher_TopicData(this, &topicData);
    }
    rtps_Unlock(&this->m_mutex);
}

void rtps_SEDPAgent_addUnmatchedRemoteWriter(struct rtps_SEDPAgent *this, const struct rtps_TopicData *writerData) {
    if (rtps_MemoryPool_isFull(this->m_unmatchedRemoteWriters)) {
#if SEDP_VERBOSE
        SEDP_LOG("List of unmatched remote writers is full.\n");
#endif
        return;
    }
    SEDP_LOG("Adding unmatched remote writer %s %s.\n", writerData->topicName, writerData->typeName);
    struct rtps_TopicDataCompressed topicDataCompressed_temp;
    TopicDataCompressed_Init(&topicDataCompressed_temp, writerData);
    rtps_MemoryPool_add(this->m_unmatchedRemoteWriters, (void*)&topicDataCompressed_temp);
}

void rtps_SEDPAgent_addUnmatchedRemoteReader(struct rtps_SEDPAgent *this, const struct rtps_TopicData *readerData) {
    if (rtps_MemoryPool_isFull(this->m_unmatchedRemoteReaders)) {
#if SEDP_VERBOSE
        SEDP_LOG("List of unmatched remote readers is full.\n");
#endif
        return;
    }
    SEDP_LOG("Adding unmatched remote reader %s %s.\n", readerData->topicName, readerData->typeName);
    struct rtps_TopicDataCompressed topicDataCompressed_temp;
    TopicDataCompressed_Init(&topicDataCompressed_temp, readerData);
    rtps_MemoryPool_add(this->m_unmatchedRemoteReaders, (void*)&topicDataCompressed_temp);
}

void rtps_SEDPAgent_onNewPublisher_TopicData(struct rtps_SEDPAgent *this, const struct rtps_TopicData *writerData) {
    // TODO Is it okay to add Endpoint if the respective participant is unknown
    struct rtps_ParticipantProxyData * proxyData = rtps_Participant_findRemoteParticipant(this->m_part, &((writerData->endpointGuid).prefix));
    if(!proxyData) {
        return;
    }
#if SEDP_VERBOSE
    SEDP_LOG("PUB T/D %s/%s", writerData->topicName, writerData->typeName);
#endif
    struct rtps_Reader *reader = rtps_Participant_getMatchingReader(this->m_part, writerData);
    if (reader == NULL) {
#if SEDP_VERBOSE
        SEDP_LOG("SEDPAgent: Couldn't find reader for new Publisher[%s, %s] \n",
                writerData->topicName, writerData->typeName);
        rtps_SEDPAgent_addUnmatchedRemoteWriter(this, writerData);
#endif
        rtps_Participant_findRemoteParticipant_free(proxyData);
        return;
    }
  // TODO check policies
#if SEDP_VERBOSE
    SEDP_LOG("Found a new ");
    if (writerData->reliabilityKind == RELIABLE) {
        SEDP_LOG("reliable ");
    } else {
        SEDP_LOG("best-effort ");
    }
    SEDP_LOG("publisher\n");
#endif

    struct rtps_WriterProxy writerProxy_temp;
    memset(&writerProxy_temp, 0 , sizeof(writerProxy_temp));
    writerProxy_temp.remoteWriterGuid = writerData->endpointGuid;
    writerProxy_temp.expectedSN.high = 0;
    writerProxy_temp.expectedSN.low  = 1;
    writerProxy_temp.ackNackCount.value = 1;
    writerProxy_temp.hbCount.value = 0;
    writerProxy_temp.remoteLocator = writerData->unicastLocator;
    if (reader->m_bstateful)
        rtps_StatefulReader_addNewMatchedWriter((struct rtps_StatefulReader *)reader, &writerProxy_temp);
    else
        rtps_StatelessReader_addNewMatchedWriter((struct rtps_StatelessReader *)reader, &writerProxy_temp);
    if (this->mfp_onNewPublisherCallback != NULL) {
        this->mfp_onNewPublisherCallback(this->m_onNewPublisherArgs);
    }
    rtps_Participant_findRemoteParticipant_free(proxyData);
}

void rtps_SEDPAgent_onNewSubscriber_ReaderCacheChange(struct rtps_SEDPAgent *this, const struct rtps_ReaderCacheChange *change) {

    rtps_Lock(&this->m_mutex);
#if SEDP_VERBOSE
  SEDP_LOG("New subscriber\n");
#endif

    int len = sizeof(this->m_buffer) / sizeof(this->m_buffer[0]);
    if(!rtps_ReaderCacheChange_copyInto(change, this->m_buffer, len)) {
#if SEDP_VERBOSE
        SEDP_LOG("SEDPAgent: Buffer too small.");
#endif
        rtps_Unlock(&this->m_mutex);
        return; 
    }
    struct ucdrBuffer cdrBuffer;
    ucdr_init_buffer(&cdrBuffer, this->m_buffer, sizeof(this->m_buffer));

    struct rtps_TopicData topicData;

    if(rtps_TopicData_readFromUcdrBuffer(&topicData, &cdrBuffer)) {
        rtps_SEDPAgent_onNewSubscriber_TopicData(this, &topicData);
    }
    rtps_Unlock(&this->m_mutex);
}

void rtps_SEDPAgent_onNewSubscriber_TopicData(struct rtps_SEDPAgent *this, const struct rtps_TopicData *readerData) {
    struct rtps_ParticipantProxyData * proxyData = rtps_Participant_findRemoteParticipant(this->m_part, 
                                                            &((readerData->endpointGuid).prefix));
    if(!proxyData) {
        return;
    }
    struct rtps_Writer *writer = rtps_Participant_getMatchingWriter(this->m_part, readerData);
#if SEDP_VERBOSE
    SEDP_LOG("SUB T/D %s/%s", readerData->topicName, readerData->typeName);
#endif
    if (writer == NULL) {
#if SEDP_VERBOSE
        SEDP_LOG("SEDPAgent: Couldn't find writer for new subscriber[%s, %s]\n",
             readerData->topicName, readerData->typeName);
        rtps_SEDPAgent_addUnmatchedRemoteReader(this, readerData);
#endif
        rtps_Participant_findRemoteParticipant_free(proxyData);
        return;
    }

  // TODO check policies
#if SEDP_VERBOSE
    SEDP_LOG("Found a new ");
    if(readerData->reliabilityKind == RELIABLE) {
        SEDP_LOG("reliable ");
    } else {
        SEDP_LOG("best-effort");
    }
    SEDP_LOG("Subscriber\n");
#endif
    if (readerData->multicastLocator.kind == LOCATOR_KIND_UDPv4) {
        struct rtps_ReaderProxy readerProxy_temp;
        rtps_ReaderProxy_Construct(&readerProxy_temp, 
            &(readerData->endpointGuid), &(readerData->unicastLocator), &(readerData->multicastLocator));
        if (writer->m_bstateful)
            rtps_StatefulWriter_addNewMatchedReader((struct rtps_StatefulWriter *)writer, (const ReaderProxy *)&readerProxy_temp);
        else
            rtps_StatelessWriter_addNewMatchedReader((struct rtps_StatelessWriter *)writer, (const ReaderProxy *)&readerProxy_temp);
    } else {
        struct rtps_ReaderProxy readerProxy_temp;
        rtps_ReaderProxy_Construct(&readerProxy_temp, &readerData->endpointGuid, &readerData->unicastLocator, NULL);
        if (writer->m_bstateful)
            rtps_StatefulWriter_addNewMatchedReader((struct rtps_StatefulWriter *)writer, (const ReaderProxy *)&readerProxy_temp);
        else
            rtps_StatelessWriter_addNewMatchedReader((struct rtps_StatelessWriter *)writer, (const ReaderProxy *)&readerProxy_temp);
    }

    if (this->mfp_onNewSubscriberCallback != NULL) {
        this->mfp_onNewSubscriberCallback(this->m_onNewSubscriberArgs);
    }
    rtps_Participant_findRemoteParticipant_free(proxyData);
}

/*====================================================*/
void rtps_SEDPAgent_tryMatchUnmatchedEndpoints(struct rtps_SEDPAgent *this) {
    uint32_t offset;
    uint8_t *ptmp, *pblk, *pdata;
    struct rtps_TopicDataCompressed *proxy;

    RTPS_MEMORYPOOL_ITER((this->m_unmatchedRemoteReaders), offset, ptmp, pblk, pdata) {
        proxy = (struct rtps_TopicDataCompressed*)pdata;
        struct rtps_Writer *writer = rtps_Participant_getMatchingWriter_compressed(this->m_part, proxy);   // need use TopicDataCompress
        if (writer) {
            struct rtps_ReaderProxy readerProxy_temp;

            memset(&readerProxy_temp, 0, sizeof(readerProxy_temp));
            readerProxy_temp.remoteReaderGuid = proxy->endpointGuid;
            readerProxy_temp.remoteLocator = proxy->unicastLocator;
            readerProxy_temp.remoteMulticastLocator = proxy->multicastLocator;
            readerProxy_temp.ackNackSet.base.high = 0;
            readerProxy_temp.ackNackSet.base.low  = 0;
            readerProxy_temp.ackNackCount.value = 0;
            if (writer->m_bstateful)
                rtps_StatefulWriter_addNewMatchedReader((struct rtps_StatefulWriter *)writer, &readerProxy_temp);
            else
                rtps_StatelessWriter_addNewMatchedReader((struct rtps_StatelessWriter *)writer, &readerProxy_temp);
        }
    }

    RTPS_MEMORYPOOL_ITER((this->m_unmatchedRemoteWriters), offset, ptmp, pblk, pdata) {
        proxy = (struct rtps_TopicDataCompressed*)pdata;
        struct rtps_Reader *reader = rtps_Participant_getMatchingReader_compressed(this->m_part, proxy);
        if (reader) {
            struct rtps_WriterProxy writerProxy_temp;

            memset(&writerProxy_temp, 0 , sizeof(writerProxy_temp));
            writerProxy_temp.remoteWriterGuid = proxy->endpointGuid;
            writerProxy_temp.remoteLocator = proxy->unicastLocator;
            writerProxy_temp.expectedSN.high = 0;
            writerProxy_temp.expectedSN.low  = 1;
            writerProxy_temp.ackNackCount.value = 1;
            writerProxy_temp.hbCount.value = 0;
            if (reader->m_bstateful)
                rtps_StatefulReader_addNewMatchedWriter((struct rtps_StatefulReader *)reader, &writerProxy_temp);
            else
                rtps_StatelessReader_addNewMatchedWriter((struct rtps_StatelessReader *)reader, &writerProxy_temp);
        }
    }
}

void rtps_SEDPAgent_addWriter(struct rtps_SEDPAgent *this, struct rtps_Writer *writer) {
    if (this->m_endpoints.sedpPubWriter == NULL) {
        return;
    }
    EntityKind_t writerKind = writer->m_attributes.endpointGuid.entityId.entityKind;
    if (writerKind == BUILD_IN_WRITER_WITH_KEY || writerKind == BUILD_IN_WRITER_WITHOUT_KEY) {
        return; // No need to announce builtin endpoints
    }

    rtps_Lock(&this->m_mutex);

    // Check unmatched writers for this new reader
    rtps_SEDPAgent_tryMatchUnmatchedEndpoints(this);

    struct ucdrBuffer microbuffer;
    ucdr_init_buffer(&microbuffer, this->m_buffer, sizeof(this->m_buffer) / sizeof(this->m_buffer[0]));
    const uint16_t zero_options = 0;

    ucdr_serialize_array_uint8_t(&microbuffer, SCHEME_PL_CDR_LE, sizeof(SCHEME_PL_CDR_LE) / sizeof(SCHEME_PL_CDR_LE[0]));
    ucdr_serialize_uint16_t(&microbuffer, zero_options);
    rtps_TopicData_serializeIntoUcdrBuffer(&(writer->m_attributes), &microbuffer);
    rtps_StatefulWriter_newChange(this->m_endpoints.sedpPubWriter, ALIVE, this->m_buffer, ucdr_buffer_length(&microbuffer));

#if SEDP_VERBOSE
    SEDP_LOG("Added new change to sedpPubWriter.\n");
#endif
    rtps_Unlock(&this->m_mutex);
}

void rtps_SEDPAgent_addReader(struct rtps_SEDPAgent *this, struct rtps_Reader *reader) {
    if (this->m_endpoints.sedpSubWriter == NULL) {
        return;
    }

    EntityKind_t readerKind = reader->m_attributes.endpointGuid.entityId.entityKind;
    if (readerKind == BUILD_IN_READER_WITH_KEY || readerKind == BUILD_IN_READER_WITHOUT_KEY) {
        return; // No need to announce builtin endpoints
    } 

    rtps_Lock(&this->m_mutex);

    // Check unmatched writers for this new reader
    rtps_SEDPAgent_tryMatchUnmatchedEndpoints(this);

    ucdrBuffer microbuffer;
    ucdr_init_buffer(&microbuffer, this->m_buffer,
                   sizeof(this->m_buffer) / sizeof(this->m_buffer[0]));
    const uint16_t zero_options = 0;

    ucdr_serialize_array_uint8_t(&microbuffer, SCHEME_PL_CDR_LE, sizeof(SCHEME_PL_CDR_LE) / sizeof(SCHEME_PL_CDR_LE[0]));
    ucdr_serialize_uint16_t(&microbuffer, zero_options);
    rtps_TopicData_serializeIntoUcdrBuffer(&reader->m_attributes, &microbuffer);
    rtps_StatefulWriter_newChange(this->m_endpoints.sedpSubWriter, ALIVE, this->m_buffer, ucdr_buffer_length(&microbuffer));

#if SEDP_VERBOSE
    SEDP_LOG("Added new change to sedpSubWriter.\n");
#endif
    rtps_Unlock(&this->m_mutex);
}
